
#include "mainwindow.h"

MainWindow::MainWindow(QWidget *parent)
	: QOpenGLWidget(parent), m_program(nullptr), m_pointCount(0)
{
	m_dataUploaded = false;
	m_projection.setToIdentity();
}

MainWindow::~MainWindow()
{
	makeCurrent();
	m_vbo.destroy();
	delete m_program;
	doneCurrent();
}

void MainWindow::setPointCloud(const QVector<QVector3D> &points, const QVector<QVector3D> &colors)
{
	m_points = points;
	m_colors = colors.isEmpty() ? QVector<QVector3D>(points.size(), QVector3D(1, 1, 1)) : colors;
	m_pointCount = points.size();
	m_dataUploaded = false;

	if (isValid())
	{
		makeCurrent();

		// 将数据上传到GPU
		m_vao.bind();
		m_vbo.bind();
		m_vbo.allocate(m_pointCount * (3 + 3) * sizeof(float));

		float *data = static_cast<float *>(m_vbo.map(QOpenGLBuffer::WriteOnly));
		for (int i = 0; i < m_pointCount; ++i)
		{
			data[i * 6 + 0] = m_points[i].x();
			data[i * 6 + 1] = m_points[i].y();
			data[i * 6 + 2] = m_points[i].z();
			data[i * 6 + 3] = m_colors[i].x();
			data[i * 6 + 4] = m_colors[i].y();
			data[i * 6 + 5] = m_colors[i].z();
		}
		m_vbo.unmap();
		m_vbo.release();
		m_vao.release();

		m_dataUploaded = true;
		doneCurrent();
		update();
	}
}

void MainWindow::onReceiveDepthFrame(QByteArray depthData)
{
	char *pData = depthData.data();

	int depthWidth = *(int *)(pData + DATA_OFFSET + 1);
	int depthHeight = *(int *)(pData + DATA_OFFSET + 5);

	int depthSize = *(qint64 *)(pData + DATA_OFFSET + 17);

	float wScale = width() * 1.0f / depthWidth;
	float hScale = height() * 1.0f / depthHeight;

	unsigned short *rawData = (unsigned short *)(pData + DATA_OFFSET + 25);

	m_pointCount = depthWidth * depthHeight;
	m_dataUploaded = false;

	if (isValid())
	{
		unsigned short maxDepth = 0;
		int size = qMin(depthWidth * depthHeight, depthSize);

		for (int i = 0; i < size; i++)
		{
			unsigned short depth = rawData[i];
            depth = ((depth & 0x00FF) << 8) + ((depth & 0xFF00) >> 8);

			if (depth > maxDepth)
			{
				maxDepth = depth;
			}
		}

		makeCurrent();

		// 将数据上传到GPU
		m_vao.bind();
		m_vbo.bind();
		m_vbo.allocate(m_pointCount * (3 + 3) * sizeof(float));

		float *data = static_cast<float *>(m_vbo.map(QOpenGLBuffer::WriteOnly));

		for (int y = 0; y < depthHeight; y++)
		{
			for (int x = 0; x < depthWidth; x++)
			{
				unsigned short depth = rawData[y * depthWidth + x];

				depth = ((depth & 0x00FF) << 8) + ((depth & 0xFF00) >> 8);

				float z = depth * 1.0f / maxDepth;
				int index = y * depthWidth + x;

				data[index * 6 + 0] = x * wScale * 2 / depthWidth - 1.0;
				data[index * 6 + 1] = y * hScale * 2 / depthHeight - 1.0;
				data[index * 6 + 2] = 0 - z;

				data[index * 6 + 3] = 200.0f / 255;
				data[index * 6 + 4] = z;
				data[index * 6 + 5] = 0;
			}
		}

		m_vbo.unmap();
		m_vbo.release();
		m_vao.release();

		m_dataUploaded = true;
		doneCurrent();
		update();
	}
}

void MainWindow::initializeGL()
{
	initializeOpenGLFunctions();

	glClearColor(0.1f, 0.1f, 0.1f, 1.0f);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_PROGRAM_POINT_SIZE);

	// 创建VAO
	m_vao.create();
	m_vao.bind();

	// 创建VBO
	m_vbo.create();
	m_vbo.setUsagePattern(QOpenGLBuffer::DynamicDraw);
	m_vbo.bind();

	// 设置顶点属性
	glEnableVertexAttribArray(0);
	glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(float), (void *)0);
	glEnableVertexAttribArray(1);
	glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(float), (void *)(3 * sizeof(float)));

	m_vbo.release();
	m_vao.release();

	// 创建着色器程序
	m_program = new QOpenGLShaderProgram(this);

	m_program->addShaderFromSourceFile(QOpenGLShader::Vertex, ":/glsl/pcd.vs");
	m_program->addShaderFromSourceFile(QOpenGLShader::Fragment, ":/glsl/pcd.fs");

	if (!m_program->link())
	{
		qDebug() << "Shader link error:" << m_program->log();
	}

	if (!m_points.isEmpty())
	{
		setPointCloud(m_points, m_colors);
	}
}

void MainWindow::resizeGL(int w, int h)
{
	m_projection.setToIdentity();
    m_projection.perspective(45.0f, GLfloat(w) / h, 0.001f, 100.0f);
}

void MainWindow::paintGL()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	if (!m_dataUploaded || m_pointCount == 0)
		return;

	m_program->bind();
	m_program->setUniformValue("projection", m_projection);

	m_vao.bind();
	glDrawArrays(GL_POINTS, 0, m_pointCount);
	m_vao.release();

	m_program->release();
}

